#include "mapping/octomap_multi_scan.h"
#include "mapping/save_2d_map.h"

bool do_save_map = false;
bool save_map_callback(mapping::save_2d_mapRequest& request,mapping::save_2d_mapResponse& response){
    LOG(INFO) << "STARTING SAVING 2D MAP.";
    do_save_map = true;
    response.success = true;
    return true;
}

int main(int argc,char** argv){
    google::InitGoogleLogging(argv[0]);
    FLAGS_alsologtostderr = true;
    FLAGS_log_dir = std::string(PROJECT_PATH) + "/log/octomap_multi_scan";

    ros::init(argc,argv,"octomap_multi_scan_node");
    ros::NodeHandle nh;
    //LOG(INFO) << "thread ID in main thread -> : " << std::this_thread::get_id();

    Gen_grid_map gen_grid_map(nh);
    ros::ServiceServer map_service_server = nh.advertiseService("mapping/save_map",save_map_callback);
    
    ros::Rate rate(100);
    while(ros::ok()){
        if(do_save_map){
            gen_grid_map.save_2d_map();
            do_save_map = false;
        }
        ros::spinOnce();
        rate.sleep();
    }
}
















